import cv2
import numpy as np
from LOBOROBOT import LOBOROBOT # 载入机器人库
import time
# 线程函数操作库
import threading  # 线程
import ctypes
import inspect
import traitlets
import ipywidgets.widgets as widgets
from IPython.display import display


clbrobot = LOBOROBOT()  # 实例化机器人对象
# Configure min and max servo pulse lengths
servo_min = 150  # Min pulse length out of 4096
servo_max = 600  # Max pulse length out of 4096
# 舵机调零
pan =  90
tilt = -10
# 频率设置为50hz，适用于舵机系统。
clbrobot.set_servo_angle(10,pan)  # 底座舵机 90
clbrobot.set_servo_angle(9,tilt)  # 顶部舵机 30
time.sleep(0.5)


def bgr8_to_jpeg(value, quality=75):
    return bytes(cv2.imencode('.jpg', value)[1])

FGmaskComp_img = widgets.Image(format='jpeg', width=320, height=240)
frame_img = widgets.Image(format='jpeg', width=320, height=240)

dispaly_img = widgets.HBox([FGmaskComp_img,frame_img])
display(dispaly_img)



# 线程结束代码
def _async_raise(tid, exctype):
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        raise SystemError("PyThreadState_SetAsyncExc failed")


def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)

dispW = 320
dispH = 240

cam = cv2.VideoCapture(0, cv2.CAP_V4L2)
cam.set(3, 320)
cam.set(4, 240)
cam.isOpened()

width = cam.get(cv2.CAP_PROP_FRAME_WIDTH)
height = cam.get(cv2.CAP_PROP_FRAME_HEIGHT)
print('width:', width, 'height:', height)

hueLower = widgets.IntSlider(min=96, max=179, step=1, description='hueLower：', value=96)
hueUpper = widgets.IntSlider(min=120, max=179, step=1, description='hueUpper：', value=120)

hue2Lower = widgets.IntSlider(min=50, max=179, step=1, description='hue2Lower：', value=50)
hue2Upper = widgets.IntSlider(min=0, max=179, step=1, description='hue2Upper：', value=0)

satLow = widgets.IntSlider(min=157, max=255, step=1, description='satLow：', value=157)
satHigh = widgets.IntSlider(min=255, max=255, step=1, description='satHigh：', value=255)

valLow = widgets.IntSlider(min=100, max=255, step=1, description='valLow：', value=100)
valHigh = widgets.IntSlider(min=255, max=255, step=1, description='valHigh：', value=255)

slider_img = widgets.VBox([
    widgets.HBox([hueLower, hueUpper]),
    widgets.HBox([hue2Lower, hue2Upper]),
    widgets.HBox([satLow, satHigh]),
    widgets.HBox([valLow, valHigh])
])
display(slider_img)


def Video_display():
    global pan
    global tilt
    while True:
        ret, frame = cam.read()
        frame = cv2.flip(frame, 0)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        hueLow = hueLower.value
        hueUp = hueUpper.value

        hue2Low = hue2Lower.value
        hue2Up = hue2Upper.value

        Ls = satLow.value
        Us = satHigh.value

        Lv = valLow.value
        Uv = valHigh.value

        l_b = np.array([hueLow, Ls, Lv])
        u_b = np.array([hueUp, Us, Uv])

        l_b2 = np.array([hue2Low, Ls, Lv])
        u_b2 = np.array([hue2Up, Us, Uv])

        FGmask = cv2.inRange(hsv, l_b, u_b)
        FGmask2 = cv2.inRange(hsv, l_b2, u_b2)
        FGmaskComp = cv2.add(FGmask, FGmask2)
        FGmaskComp_img.value = bgr8_to_jpeg(FGmaskComp)

        contours, _ = cv2.findContours(FGmaskComp, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)

        for cnt in contours:
            area = cv2.contourArea(cnt)
            (x, y, w, h) = cv2.boundingRect(cnt)
            if area >= 50:
                cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 3)
                objX = x + w / 2
                objY = y + h / 2

                errorPan = objX - width / 2
                errorTilt = objY - height / 2

                if abs(errorPan) > 15:
                    pan = pan - errorPan / 75

                if abs(errorTilt) > 15:
                    tilt = tilt - errorTilt / 75

                if pan > 90:
                    pan = 90
                    print("Pan Out of  Range")
                    # clbrobot.set_servo_angle(10,90)
                if pan < -90:
                    pan = 90
                    print("Pan Out of  Range")
                    # clbrobot.set_servo_angle(10,90)
                if tilt > 90:
                    tilt = 90
                    print("Tilt Out of  Range")
                if tilt < -90:
                    tilt = 90
                    print("Tilt Out of  Range")

                clbrobot.set_servo_angle(10, 90 - pan)
                clbrobot.set_servo_angle(9, 90 - tilt)
                break
            else:
                clbrobot.set_servo_angle(10, 90)  # 底座舵机 90
                clbrobot.set_servo_angle(9, 0)  # 顶部舵机 30

        frame_img.value = bgr8_to_jpeg(frame)
    cam.release()
t = threading.Thread(target=Video_display)
t.setDaemon(True)
t.start()
stop_thread(t)